//(C) 2011 Hannes Hassler. 
//May be used under 
//GNU GENERAL PUBLIC LICENSE (GPL3)
//http://www.gnu.org/licenses/gpl-3.0.html



#include <SensorAktor.h>
#include <SoftwareServo.h>

#include <ctype.h>

#define bit9600Delay 84  
#define halfBit9600Delay 42
#define bit4800Delay 188
#define halfBit4800Delay 94

#define rstPin 13

#define NoNl     false
#define Nl       true

#define REFRESH_RATE 300


#define refreshCountdown 1000


#include "Wire.h"
#include <Helios.h>
#include <DS1107.h>



SoftwareServo azimuth;
SoftwareServo elevation;
int winkel;
int winkelAzimuth;
int winkelElevation;


Helios helios;
DS1107 clock;


double dAzimuth;
double dElevation;
      
char ch;

//to set the timer chip
//Upload it with s
byte second=0;
byte minute=45;
byte hour=8;
byte dayOfWeek=6;
byte dayOfMonth=21;
byte month=5;
byte year=11;

//Vienna coordinates
float localLongitude=16.3666;
float localLatitude=48.2;

byte servoElevation=7;
byte servoAzimuth=6;
byte servoSpeed=3;

byte currentAzPos=0;
byte newAzPos=1;
byte currentElvPos=0;
byte newElvPos=1;

float k_az=0.65833;
int d_az=41;

float k_elv=0.6833;
int d_elv=130;

int refreshCountdownIndex=refreshCountdown;

boolean showServoResponse=false;
boolean showPololuReset=false;

unsigned long millisSinceLastServoRefresh=0;

byte debugShow=0;

void setup()
{
   
      
    pinMode(rstPin, OUTPUT);
    resetPololu();    
    
        
    Serial.begin(9600);
    Serial3.begin(20000);
  
    set_speed(servoElevation,servoSpeed);
    set_speed(servoAzimuth,servoSpeed);
    
    initPositionHeliostat();
  
}



void loop()
{

 static int v = 0; 

 if (Serial.available()) { 
      ch = Serial.read();  
      switch(ch) {
     case '0'...'9':
        v = v * 10 + ch - '0';
        break;
      case 'E':
        show("elevation val:",v,Nl); 
        set_speed(servoElevation,servoSpeed);        
        position_8bit(servoElevation,v);       
        v = 0; break;
      case 'A':
        show("azimuthVal:",v,Nl);  
        set_speed(servoAzimuth,servoSpeed);       
        position_8bit(servoAzimuth,v); 
        v = 0; break;      
      case 'c':   
       clock.setDate(second,minute,hour,dayOfWeek,dayOfMonth,month,year);
       break;
      case 'd':
       show("set hour:", v, Nl);
       hour=v;
       v=0;break;
      case 'f':
       show("set minute:", v, Nl);
       minute=v;
       v=0;break;
      case 'a':
       show("set d_az:",v,Nl);
       d_az=v; 
       v=0; break;
      case 'e':
       show("set d_elv:",v,Nl);
       d_elv=v;
       v=0; break;
      case 'p':        
        clock.getDate();  
        helios.calcSunPos(2000+clock.year,clock.month,clock.dayOfMonth,
        clock.hour, clock.minute,clock.second,localLongitude,localLatitude); 
        showTime(clock);
        dAzimuth=helios.dAzimuth;show("dAzimuth",dAzimuth,true);
        dElevation=helios.dElevation;show("dElevation",dElevation,true);
        showAzElv();
       break;   
      case 'q':
         showServoResponse=false;
         Serial.println("showServoResponse=false");
         break;
      case 'r':
         showServoResponse=true;
         Serial.println("showServoResponse=true");
         break;
      case 's':
         resetPololu();
         Serial.println("resetPololu");
         break;
      case 't':
         showPololuReset=true;
         Serial.println("showPololuReset=true");
         break;
      case 'u':
         showPololuReset=false;
         Serial.println("showPololuReset=false");
         break;
    }
  }  

    
  positionHeliostat();  
  
}


void sendNum(int num) {
  Serial.print(num);  
}

void resetPololu() {
  
  digitalWrite(rstPin, LOW); //resets the Pololu board
  delay(100);
  digitalWrite(rstPin, HIGH);
  if (showPololuReset) {
    Serial.println("Pololu resetted");
  }
  
}

void show(char nameStr[], double val, boolean newline) {
  Serial.print(nameStr);  
  Serial.print("=");
  if (newline)
       Serial.println(val);
  else Serial.print(val);
}

void showAzElv() {
  
   show("d_az:",d_az,Nl);
   show("d_elv:",d_elv,Nl);
   show("azPos:",currentAzPos,Nl);
   show("elvPos:",currentElvPos,Nl);
  
}

void showTime(DS1107 timerChip) {
  Serial.print("UT ");
  Serial.print(timerChip.hour, DEC);
  Serial.print(":");
  Serial.print(timerChip.minute, DEC);
  Serial.print(":");
  Serial.print(timerChip.second, DEC);
  Serial.print("  ");
  Serial.print(timerChip.month, DEC);
  Serial.print("/");
  Serial.print(timerChip.dayOfMonth, DEC);
  Serial.print("/");
  Serial.print(timerChip.year, DEC);
  Serial.print("  Day_of_week:");
  Serial.println(timerChip.dayOfWeek, DEC);  
}

void initPositionHeliostat() {
  
    for (int i=0; i<3;i++) {
      positionHeliostat(); 
      delay(REFRESH_RATE);
      currentAzPos--;
      currentElvPos--;    
    }
}

void positionHeliostat() {
  
   
   unsigned long  currentMillis = millis();
   boolean doRefresh=false; 
   
   if ((currentMillis-millisSinceLastServoRefresh)>REFRESH_RATE) {
     
     doRefresh=true;
     millisSinceLastServoRefresh=currentMillis;
       
     
     clock.getDate();  
     helios.calcSunPos(2000+clock.year,clock.month,clock.dayOfMonth,
     clock.hour, clock.minute,clock.second,localLongitude,localLatitude); 
   
     dAzimuth=helios.dAzimuth;
     dElevation=helios.dElevation;
   
     newAzPos=(k_az*dAzimuth)+d_az;
     newElvPos=(k_elv*dElevation)+d_elv; 
       
   }
   
   if (newAzPos!=currentAzPos || newElvPos!=currentElvPos || doRefresh) {        
              
        if (newAzPos!=currentAzPos || doRefresh) {            
            if(newAzPos!=currentAzPos) {
                show("newAzPos:",newAzPos,Nl); 
            } 
            currentAzPos=newAzPos;            
            position_8bit(servoAzimuth,currentAzPos);  
            delay(300);            
        }
        
        
       
        
        if (newElvPos!=currentElvPos || doRefresh) {
            if(newElvPos!=currentElvPos) {
              show("newElvPos:",newElvPos,Nl);              
            }
            currentElvPos=newElvPos;            
            position_8bit(servoElevation,currentElvPos);  
        }
                
        
   }
   
        
}




void position_8bit(byte servo, byte posValue)
{
  //this function uses pololu mode command 3 to set position
  //servo is the servo number (typically 0-7)
  //posValue * range (set with command 0) adjusted by neutral (set with command 5)
  //determines servo position

   byte temp;
   byte pos_hi,pos_low;
   int pos_hiInt, pos_lowInt;
   byte ch;
   int chInt;
   
   int sendTries=3;
   int noOfResponses[sendTries];
   

   temp = posValue & 0x80;	//get bit 8 of position
   pos_hi = temp >> 7;		//shift bit 8 by 7
   pos_hiInt=pos_hi;
   pos_low = posValue & 0x7f;     //get lower 7 bits of position
   pos_lowInt=pos_low;
   
   
   //Send a Pololu Protocol command
   if (showServoResponse) {
     Serial.print("send to servo:");
     Serial.print(pos_hiInt);
     Serial.print(" ");
     Serial.print(pos_lowInt);
     Serial.print("\n");
   }
   
   
   for (int i=0; i<sendTries;i++) {
     Serial3.print(0x80,BYTE);     //start byte
     Serial3.print(0x01,BYTE);     //device id
     Serial3.print(0x03,BYTE);     //command number
     Serial3.print(servo,BYTE);    //servo number
     Serial3.print(pos_hi, BYTE);  //bits 8 thru 13
     Serial3.print(pos_low, BYTE); //bottom 7 bits
   
     noOfResponses[i]=0;
     while (Serial3.available()) { 
        noOfResponses[i]++;
        ch = Serial3.read();
        chInt=ch;
        //if (chInt==0) {resetPololu();}
        if (showServoResponse) {
           Serial.print(chInt);
           Serial.print(" ");
        }
     }      
     if (showServoResponse) {
         Serial.print("\n");
     }
     if (chInt==pos_lowInt) {
         //Serial.print("OK\n\n");
     }
   }
   
   
   if (showServoResponse) {
         Serial.print("Sendtries: ");
   }
   boolean oneCorrectlyReceived=false;
   for (int i=0; i<sendTries;i++) {
     oneCorrectlyReceived=oneCorrectlyReceived || (noOfResponses[i]==6);
     if (showServoResponse) {
         Serial.print(noOfResponses[i]);
         Serial.print(" ");
     }
     
   }
   if (showServoResponse) {
         Serial.print("\n");
   }
   if (!oneCorrectlyReceived) {resetPololu();}
   
   
}

void set_speed(byte servo, byte speedVal)
{
   //this function uses pololu mode command 1 to set speed
   //servo is the servo number (typically 0-7)
   //speedVal is servo speed (1=slowest, 127=fastest, 0=full)
   //set speedVal to zero to turn off speed limiting

   speedVal = speedVal & 0x7f; //take only lower 7 bits of the speed

   //Send a Pololu Protocol command
   Serial3.print(0x80,BYTE);     //start byte
   Serial3.print(0x01,BYTE);     //device id
   Serial3.print(0x01,BYTE);     //command number
   Serial3.print(servo,BYTE);    //servo number
   Serial3.print(speedVal,BYTE); //speed
}
